// ==============================================================================================
// BSD 3-Clause Clear License
// Copyright © 2025 ZAMA. All rights reserved.
// ----------------------------------------------------------------------------------------------
// ucore firmware
//
// Continuously:
// *  Read IOPs from work queue
// *  Parse them to extract OpCode and src/dest ops
// *  Read corresponding DOps stream from translation table
// *  If DOp src/dst match template pattern, patch Opcode with real IOps args
// *  Send patched DOps to HW
//
// Behind the scene, an interrput handler wait on Hw DOps ack and forward them to host throught
// the ack queue.
// ==============================================================================================

#include "profile_hal.h"
#include "ucore.h"

// Ops functions body
// ============================================================================================= //
// Parse an IOp from stream
// returns the number of bytes used by parsed IOp
// Currently there is no way to report error back to host -> No check are implemented during parsing
// TODO implement error return through ACKQ and implement check during parsing
uint32_t parse_iop(
     uint32_t *stream,
     uint32_t iop_pending_bytes,
     // Static allocated buffer used during IOp parsing
     IOpHeader_t *header,
     IOpOperand_t *operand,
     IOpImmHeader_t *imm_header,
     // Operand/Immediat bundle generated by the parser
     OperandBundle_t* dst,
     OperandBundle_t* src,
     ImmediatBundle_t* imm) {

  // TODO Correctly invalidate cache_data
  // Should be handled outside of the function by the GCQ reader
  // HAL_INVALIDATE_CACHE_DATA( (uintptr_t)(stream), bytes_len);

  uint32_t stream_pos = 0;
  //1. Get header
  header->raw = stream[stream_pos];
  stream_pos++;

  if ((stream_pos*sizeof(uint32_t)) > iop_pending_bytes) {
    PLL_ERR("parse_iop", "not enough bytes after header");
    return 0;
  }

  //2. Get list of destination operands
  uint32_t dst_pos = 0;
  do {
    operand->raw = stream[stream_pos];
    stream_pos++;
    if ((stream_pos*sizeof(uint32_t)) > iop_pending_bytes) {
      for (int i =0; i < 7; i++) {
          PLL_ERR("parse_iop", "Fail parse_iop dsts", "@%d -> 0x%x", i, Xil_EndianSwap32(stream[i]));
      }
      PLL_ERR("parse_iop", "not enough bytes to reach last destination");
      return 0;
    }

    // Fill current slot
    dst->operand[dst_pos].cid_ofst = operand->operand.base_cid;
    dst->operand[dst_pos].len = operand->operand.vec_size +1;
    dst_pos +=1;
  } while (!operand->operand.is_last);

  // Fill bundle length
  dst->len = dst_pos;

  //3. Get list of source operands
  uint32_t src_pos = 0;
  do {
    operand->raw = stream[stream_pos];
    stream_pos++;

    if ((stream_pos*sizeof(uint32_t)) > iop_pending_bytes) {
      for (int i =0; i < 7; i++) {
          PLL_ERR("parse_iop", "Fail parse_iop srcs", "@%d -> 0x%x", i, Xil_EndianSwap32(stream[i]));
      }
      PLL_ERR("parse_iop", "not enough bytes to reach last source");
      return 0;
    }

    // Fill current slot
    src->operand[src_pos].cid_ofst = operand->operand.base_cid;
    src->operand[src_pos].len = operand->operand.vec_size +1;
    src_pos +=1;
  } while (!operand->operand.is_last);

  // Fill bundle length
  src->len = src_pos;

  //4. Get list of immediat operands (if needed)
  if (header->header.has_imm) {
    uint32_t imm_pos = 0;
    do {
      // Read Imm header
      imm_header->raw =stream[stream_pos];
      stream_pos++;

      if ((stream_pos*sizeof(uint32_t)) > iop_pending_bytes) {
        for (int i =0; i < 7; i++) {
            PLL_ERR("parse_iop", "Fail parse_iop imms", "@%d -> 0x%x", i, Xil_EndianSwap32(stream[i]));
        }
        PLL_ERR("parse_iop", "not enough bytes to reach last immediate");
        return 0;
      }

      // Extract lsb from header
      imm->cst[imm_pos].msg[0] = imm_header->header.lsb_msg;

      uint32_t msg_pos = 1;
      while ( imm_header->header.block > ((8*sizeof(uint16_t))/MSG_WIDTH)*msg_pos) {
        uint32_t bfr;
        bfr =stream[stream_pos];
        stream_pos++;

        // Fill current slot
        imm->cst[imm_pos].msg[msg_pos] = bfr & 0xffff;
        imm->cst[imm_pos].msg[msg_pos+1] = (bfr>> 16) & 0xffff;
        msg_pos +=2;
      };
      // Fill immediat length
      imm->cst[imm_pos].len = msg_pos;

      imm_pos += 1;
    } while (!imm_header->header.is_last);
    // Fill immediat bundle length
    imm->len = imm_pos;
  }
  return (stream_pos*sizeof(uint32_t));
}

uint32_t get_lookup(IOpHeader_t header, Lookup_t* lookup){
  // Read translation offset for the given entry
  // Offset is computed based on max blk_width to correctly handle asym IOp such as Cmp
  uint8_t max_align = (header.header.dst_align > header.header.src_align)? header.header.dst_align: header.header.src_align;
  if (max_align > 127) {
    PLL_ERR("get_lookup", "max_align is wrong src %d dst %d", header.header.src_align, header.header.dst_align);
    lookup->len = 0;
    lookup->ptr = NULL;
    return 1;
  }
  uintptr_t entry_addr = (uintptr_t) (DOP_LUT_ADDR + ((DOP_LUT_RANGE *max_align) + (header.header.opcode)) * sizeof(uint32_t));
  if ( (entry_addr < DOP_LUT_ADDR) ||
       (entry_addr > (DOP_LUT_ADDR + 0x20000))) {
    PLL_ERR("get_lookup", "entry_addr is not in expected range %x, max_align %d, opcode %d header %x", entry_addr, max_align, header.header.opcode, header.raw);
    lookup->len = 0;
    lookup->ptr = NULL;
    return 1;
  }
  HAL_INVALIDATE_CACHE_DATA(entry_addr, sizeof(uint32_t));
  size_t entry = *((uint32_t* ) entry_addr);
  if ( (entry < 0x20000) ||
       (entry > 0x2000000) ) {
    PLL_ERR("get_lookup", "entry offset if wrong %x", entry);
    lookup->len = 0;
    lookup->ptr = NULL;
    return 1;
  }

  // Each translation slot start with translation unit length
  // Invalidate the associated word from the cache, retrieved the entry length
  // Then invalidate the translation slot entry
  HAL_INVALIDATE_CACHE_DATA( (uintptr_t) (DOP_LUT_ADDR + entry), sizeof(uint32_t));
  lookup->len = *((volatile uint32_t*) (DOP_LUT_ADDR + entry));
  lookup->ptr =  (volatile uint32_t*) (DOP_LUT_ADDR + entry + sizeof(uint32_t));
  HAL_INVALIDATE_CACHE_DATA( (uintptr_t) lookup->ptr, lookup->len*sizeof(uint32_t));

  return 0;
}



// Patching function
// ============================================================================================= //
// Patch templated memory instruction
// NB: IOp have variable destination and source operands
// TODO Add error handling for out_of_range patching
void patch_mem_dop(DOpu_t *dop, OperandBundle_t *iop_dst, OperandBundle_t *iop_src) {

  switch (dop->mem.mode) {
    case MEM_ADDR: { // Already an explicit ADDR -> No need to patch
      break;}
    case MEM_HEAP: { // Heap template
      // Replace Heap offset by concrete addr and toggle the mode
      dop->mem.slot = HEAP_START_SLOT - dop->mem.slot;
      dop->mem.mode = MEM_ADDR;
      break;
    }
    case MEM_SRC: { // Src template
      // Replace mem (tid,bid) by concrete addr and toggle the mode
      uint8_t tid = (dop->mem.slot >> 8) & 0xff;
      uint8_t bid = dop->mem.slot & 0xff;
      dop->mem.slot = iop_src->operand[tid].cid_ofst + bid;
      dop->mem.mode = MEM_ADDR;
      break;
    }
    case MEM_DST: { // Dst template
      // Replace mem (tid,bid) by concrote addr and toggle the mode
      uint8_t tid = (dop->mem.slot >> 8) & 0xff;
      uint8_t bid = dop->mem.slot & 0xff;
      dop->mem.slot = iop_dst->operand[tid].cid_ofst + bid;
      dop->mem.mode = MEM_ADDR;
      break;
    }
  }
}

// Patch arith message instruction
// TODO Add error handling for out_of_range patching
void patch_imm_dop(DOpu_t *dop, ImmediatBundle_t *iop_imm) {
  switch (dop->arith_msg.msg_mode) {
    case IMM_CST: { // Already an explicit CONSTANT -> No need to patch
      break;}
    case IMM_VAR: { // Immediat template
      // Replace imm (tid,bid) by concrete constant and toggle the mode
      uint8_t tid = (dop->arith_msg.msg_cst >> 8) & 0xff;
      uint8_t bid = dop->arith_msg.msg_cst & 0xff;
      // Immediat value are packed uint16_t array
      // Compute slot_id and offset
      uint8_t slot_id = (bid*MSG_WIDTH) / (8* sizeof(uint16_t));
      uint8_t offset = (bid*MSG_WIDTH) % (8* sizeof(uint16_t));

      // TODO: Let crop be configured by user ?
      // In theory we could add up to MSG_WIDTH + CARRY_WIDTH. Current crop may be to strong in some cases.
      dop->arith_msg.msg_cst = ((iop_imm->cst[tid].msg[slot_id]) >> offset) & ((1 << MSG_WIDTH) -1);
      dop->arith_msg.msg_mode = IMM_CST;
      break;
    }
  }
}

// Global template patching
// ============================================================================================= //

// Utilities function to patch DOp
void patch_dop(DOpu_t *dop,
               OperandBundle_t *dst,
               OperandBundle_t *src,
               ImmediatBundle_t *imm) {
  DOpKind_t kind = get_kind(dop);

  switch (kind) {
    case DOPK_MEM: {
      patch_mem_dop(dop, dst, src);
      break;
    }
    case DOPK_ARITH: {
      // Check if its a scalar arith operation
      if ((dop->raw_field.opcode & IMM_FLAG) == IMM_FLAG) {
        patch_imm_dop(dop, imm);
      }
      break;
    }
    case DOPK_SYNC:
    case DOPK_PBS: { // Nothing to do
      break;
    }
  }
}

// Utilites function
// ============================================================================================= //
// Convenience function to extract kind for DOp union
DOpKind_t get_kind(DOpu_t *dop) {
  uint8_t opcode = dop->raw_field.opcode;

  return ((DOpKind_t) (opcode >> 4));
}

